#ifndef IMU_H
#define IMU_H 1

#define IMU_HISTORY 2

#define IMU_MIN_SENSOR_REST 35
// gyro max rate ~36Hz, not corresponding to the above 35ms loop

#include "mathtools.h"
#include "imuprocessor.h"


class L3G4200D;
class LSM303A;
class LSM303M;
class IMU
{
  public:
    IMU();
    ~IMU();

    // setup sensors
    void setup();
 
    // measure initial position. setup filter vars
    void initialize();

    // read sensor data
    void read(); 

    void rotateToZeroFrame(Vector3D &arg);

    // execute filters
    void process();

    

    Vector3D getM();
    Vector3D getA();
    Vector3D getG();
    double   getT(); // in useconds


    IMUProcessor imup;

  private:
    L3G4200D * device_gyr;
    LSM303M  * device_mag;
    LSM303A  * device_acc;


    Quaternion baserot;
    Quaternion baseroterr;

    Vector3D avga, avgm, avgg;

    Vector3D m, a, g;

    unsigned long t;
    unsigned long prev_t;

    double facta, factm;



    // estimate raw position using acceleration only
    //void estimateAngleSimple(/*inputs*/const Vector3D &acc, const Vector3D &accerr, /*outputs*/ Quaternion &angle, Quaternion &angleerr);

    // estimate position using newton method
    //void estimateAngle(/* outputs*/ Quaternion &angle, Quaternion &angelerr);

    


};

#endif
